#include "agv_wheel.h"
#include "stdlib.h"
#include "string.h"
#include "../../bsp/bsp.h"
#include "../common/device_manager.h"
#include "../common/common_motor.h"

static int agv_wheel_init(struct agv_wheel *wheel)
{
    if (wheel == NULL || wheel->motor == NULL || wheel->motor->inited == 0) 
        return -1;
    int ret = wheel->motor->ops->init(wheel->motor);
    ret |= wheel->motor->ops->speed_set(wheel->motor, 0);
    ret |= wheel->motor->ops->direction_set(wheel->motor, wheel->motor_reverse == 1 ? motor_reverser : motor_forward);
    if (ret != 0) return -1;
    return 0;
}

static int agv_wheel_forward(struct agv_wheel *wheel)
{
    if (wheel == NULL || wheel->motor == NULL || wheel->motor->inited == 0) {
        return -1;
    }
    motor_direction_t motor_dir = (wheel->motor_reverse == 1) ? motor_reverser : motor_forward;
    int ret = wheel->motor->ops->direction_set(wheel->motor, motor_dir);
    if (ret != 0) return -2;
    wheel->direction = 1;
    ret = wheel->motor->ops->rotation_start(wheel->motor);
    if (ret != 0) return -3;
    return 0;
}

static int agv_wheel_backoff(struct agv_wheel *wheel)
{
    if (wheel == NULL || wheel->motor == NULL || wheel->motor->inited == 0) {
        return -1;
    }
    motor_direction_t motor_dir = (wheel->motor_reverse == 1) ? motor_forward : motor_reverser;
    int ret = wheel->motor->ops->direction_set(wheel->motor, motor_dir);
    if (ret != 0) return -2;
    wheel->direction = 0;
    ret = wheel->motor->ops->rotation_start(wheel->motor);
    if (ret != 0) return -3;
    return 0;
}

static int agv_wheel_stop(struct agv_wheel *wheel)
{
    if (wheel == NULL || wheel->motor == NULL || wheel->motor->inited == 0) {
        return -1;
    }
    int ret = wheel->motor->ops->rotation_stop(wheel->motor);
    if (ret != 0) return -2;
    return 0;
}

static int agv_wheel_set_speed(struct agv_wheel *wheel, unsigned short speed)
{
    if (wheel == NULL || wheel->motor == NULL || wheel->motor->inited == 0) {
        return -1;
    }
    float motor_speed = 0;
    float pi = 3.1415926;
    motor_speed = ((speed) / (2 * pi * wheel->radius)) * wheel->speed_ratio * 60;     // 转换为rpm
    motor_rpm_t rpm = (motor_rpm_t)motor_speed;
    int ret = wheel->motor->ops->speed_set(wheel->motor, rpm);
    if (ret == 0) wheel->speed = rpm;
    return ret;
}

static int agv_wheel_set_direction(struct agv_wheel *wheel, unsigned char dir)
{
    if (wheel == NULL || wheel->motor == NULL || wheel->motor->inited == 0) {
        return -1;
    }
    motor_direction_t motor_dir = (dir == 1) ? motor_forward : motor_reverser;
    if (wheel->motor_reverse) {
        motor_dir = (motor_dir == motor_forward) ? motor_reverser : motor_forward;
    }
    if (0 != wheel->motor->ops->direction_set(wheel->motor, motor_dir)) return -2;
    return 0;
}

static int agv_wheel_set_emerg(struct agv_wheel *wheel, unsigned char emerg)
{
    if (wheel == NULL || wheel->motor == NULL || wheel->motor->inited == 0) {
        return -1;
    }
    int ret = 0;
    if (emerg == 1) ret = wheel->motor->ops->emerg_enable(wheel->motor);
    else ret = wheel->motor->ops->emerg_enable(wheel->motor);
    if (0 != ret) return -2;
    return 0;
}

static int agv_wheel_get_distance(struct agv_wheel *wheel, int *distance)
{
    *distance = 0;
    return 0;
}

static int agv_wheel_get_error(struct agv_wheel *wheel, unsigned int *error)
{
    if (wheel == NULL || wheel->motor == NULL || wheel->motor->inited == 0) {
        return -1;
    }
    int ret = wheel->motor->ops->error_get(wheel->motor, error);
    return ret == 0 && *error == 0 ? 0 : -1;
}

static int agv_wheel_clear_error(struct agv_wheel *wheel)
{
    if (wheel == NULL || wheel->motor == NULL || wheel->motor->inited == 0) {
        return -1;
    }
    int ret = wheel->motor->ops->error_clear(wheel->motor);
    return ret == 0 ? 0 : -1;
}


static int agv_wheel_constructor(struct agv_wheel *wheel)
{
    if (wheel == NULL) return -1;
    wheel->init = agv_wheel_init;
    wheel->forward = agv_wheel_forward;
    wheel->backoff = agv_wheel_backoff;
    wheel->stop = agv_wheel_stop;
    wheel->set_speed = agv_wheel_set_speed;
    wheel->set_emerg = agv_wheel_set_emerg;
    wheel->get_distance = agv_wheel_get_distance;
    wheel->get_error = agv_wheel_get_error;
    wheel->clear_error = agv_wheel_clear_error;
    return 0;
}

struct agv_wheels xp_agv_wheels = {
    .reverse = 0,
    .left_wheel = {
        .motor_reverse = 1,
        .direction = 0,
        .speed_ratio = 21,
        .speed = 0,
        .radius = 9.2,
        .init = agv_wheel_init,
        .forward = agv_wheel_forward,
        .backoff = agv_wheel_backoff,
        .stop = agv_wheel_stop,
        .set_speed = agv_wheel_set_speed,
        .set_emerg = agv_wheel_set_emerg,
        .get_distance = agv_wheel_get_distance,
        .get_error = agv_wheel_get_error,
        .clear_error = agv_wheel_clear_error,
        .motor = NULL,
    },
    .right_wheel= {
        .motor_reverse = 0,
        .direction = 0,
        .speed_ratio = 21,
        .speed = 0,
        .radius = 9.2,
        .init = agv_wheel_init,
        .forward = agv_wheel_forward,
        .backoff = agv_wheel_backoff,
        .stop = agv_wheel_stop,
        .set_speed = agv_wheel_set_speed,
        .set_emerg = agv_wheel_set_emerg,
        .get_distance = agv_wheel_get_distance,
        .get_error = agv_wheel_get_error,
        .clear_error = agv_wheel_clear_error,
        .motor = NULL,
    }
};

static int xp_agv_init(struct agv_wheels *agv, struct xp_motor *l_motor, struct xp_motor *r_motor)
{
    if (agv == NULL || l_motor == NULL || r_motor == NULL) return -1;
    if (l_motor->inited == 0 || r_motor->inited == 0) return -1;
    agv->reverse = 0;
    agv->left_wheel.motor = l_motor;
    agv->right_wheel.motor = r_motor;
    agv->left_wheel.init(&agv->left_wheel);
    agv->right_wheel.init(&agv->right_wheel);
}

extern int xp_agv_wheels_init()
{
    struct xp_modbus_device *left_dev = get_modbus_device_by_sid(1);
    struct xp_modbus_device *right_dev = get_modbus_device_by_sid(2);

    static struct xp_motor left_motor = {0};
    static struct xp_motor right_motor = {0};

    xp_hcfa_speed_motor_constructor(&left_motor, left_dev, "agv_leftwheel");
    xp_hcfa_speed_motor_constructor(&right_motor, right_dev, "agv_rightwheel");
    if (left_motor.inited != 1 || right_motor.inited != 1) {
        println("xp_agv_wheels_init faild.");
        return -1;
    }
    xp_agv_init(&xp_agv_wheels, &left_motor, &right_motor);

}

int xp_wheel_set_wheel_speed(float line_speed0, float line_speed1)
{
    float left_speed = (line_speed0 >= 0) ? line_speed0 : (0-line_speed0);
    float right_speed = (line_speed1 >= 0) ? line_speed1 : (0-line_speed1);
    char left_dir = (line_speed0 >= 0) ? 0 : 1;
    char right_dir = (line_speed1 >= 0) ? 0 : 1;
    left_speed = (left_speed < 0.0001) ? 0 : left_speed*100;
    right_speed = (right_speed < 0.0001) ? 0 : right_speed*100;     // 由米每秒转为厘米每秒

    // 限定最大速度为1m/s
    left_speed = left_speed > 100 ? 100 : left_speed;
    right_speed = right_speed > 100 ? 100 : right_speed;

    // 错误检查
    if (xp_agv_wheels.left_wheel.get_error(&xp_agv_wheels.left_wheel, &error) != 0 ||
        xp_agv_wheels.right_wheel.get_error(&xp_agv_wheels.right_wheel, &error) != 0) {
        if (0 != xp_agv_wheels.left_wheel.clear_error(&xp_agv_wheels.left_wheel) &&
            0 != xp_agv_wheels.right_wheel.clear_error(&xp_agv_wheels.right_wheel)) return -1;
    }

    if (left_speed == 0 && right_speed == 0) {
        xp_agv_wheels.left_wheel.stop(&xp_agv_wheels.left_wheel);
        xp_agv_wheels.right_wheel.stop(&xp_agv_wheels.right_wheel);
        return 0;
    }

    int ret = xp_agv_wheels.left_wheel.set_speed(&xp_agv_wheels.left_wheel, left_speed);
    ret |= xp_agv_wheels.right_wheel.set_speed(&xp_agv_wheels.right_wheel, right_speed);
    if (0 != ret) {
        xp_agv_wheels.left_wheel.stop(&xp_agv_wheels.left_wheel);
        xp_agv_wheels.right_wheel.stop(&xp_agv_wheels.right_wheel);
        return -1;
    }

    if (left_dir == 0) ret |= xp_agv_wheels.left_wheel.forward(&xp_agv_wheels.left_wheel);
    else ret |= xp_agv_wheels.left_wheel.backoff(&xp_agv_wheels.left_wheel);
    if (right_dir == 0) ret |= xp_agv_wheels.right_wheel.forward(&xp_agv_wheels.right_wheel);
    else ret |= xp_agv_wheels.right_wheel.backoff(&xp_agv_wheels.right_wheel);
    if (0 != ret) {
        xp_agv_wheels.left_wheel.stop(&xp_agv_wheels.left_wheel);
        xp_agv_wheels.right_wheel.stop(&xp_agv_wheels.right_wheel);
        return -2;
    }

    return 0;
}

static float agv_line_speed = 0;
int xp_agv_wheel_debug(char *type, char *fun, char *param)
{
    if(strcmp(type,"mini") != 0)return 0;
    
    if(strcmp(fun,"agv_run") == 0) {
        float speed = atof(param);
        agv_line_speed = speed;
        xp_wheel_set_wheel_speed(agv_line_speed, agv_line_speed);
    } else if (strcmp(fun,"agv_turn_left") == 0) {
        xp_wheel_set_wheel_speed(agv_line_speed/2, agv_line_speed);
    } else if (strcmp(fun,"agv_turn_right") == 0) {
        xp_wheel_set_wheel_speed(agv_line_speed, agv_line_speed/2);
    } else if (strcmp(fun,"agv_stop") == 0) {
        xp_wheel_set_wheel_speed(0, 0);
    } else if (strcmp(fun,"agv_round_stop") == 0) {
        xp_wheel_set_wheel_speed(0, 0);
    } else if (strcmp(fun,"agv_round_left") == 0) {
        xp_wheel_set_wheel_speed(0-agv_line_speed, agv_line_speed);
    } else if (strcmp(fun,"agv_round_right") == 0) {
        xp_wheel_set_wheel_speed(agv_line_speed, 0-agv_line_speed);
    } else if (strcmp(fun,"get_warn") == 0) {
        
    } else {
        return 0;
    }
    return 1;
}

